#include "PLPlanarPolygon3dsOptimization.h"
#include "PLVertex.h"

#include <algorithm>
#include <iostream>

using std::cout;
using std::endl;
using std::make_pair;
using std::map;
using std::max;
using std::set;
using std::string;
using std::vector;

const PLuint R = 2U;
const PLuint G = 1U;
const PLuint B = 0U;

/**
  *	Overloaded ()-operator for the PLAdjacentVertexPairCompare class. Before the
  * comparison, both for first and second the addresses of the PLVertex objects
  * are sorted ascending. After that, the mFirst members are compared, then
  * the mSecond members. Only when one value of first is smaller than the
  * corresponding value of second, true is returned, false otherwise.
  *
  * @param first const reference to the first PLAdjacentVertexPair object in the comparison
  * @param second const reference to the second PLAdjacentVertexPair object in the comparison
  * @return only when one value of first is smaller than the corresponding value of second, true is returned, false otherwise
  */
bool PLAdjacentVertexPairCompare::operator()(const PLAdjacentVertexPair &first, const PLAdjacentVertexPair &second) const
{
	const PLVertex *begin[2] = { first.mFirst, first.mSecond };
	const PLVertex *end[2] = { second.mFirst, second.mSecond };

	const PLVertex *helper;

	if (begin[0] > begin[1])
	{
		helper = begin[0];
		begin[0] = begin[1];
		begin[1] = helper;
	}

	if (end[0] > end[1])
	{
		helper = end[0];
		end[0] = end[1];
		end[1] = helper;
	}

	if (begin[0] < end[0])
	{
		return true;
	}
	else if (begin[0] > end[0])
	{
		return false;
	}
	else
	{
		if (begin[1] < end[1])
		{
			return true;
		}
		else
		{
			return false;
		}
	}
}

/**
  *	Overloaded ()-operator for the PLAdjacentVertexPairCompare class. Before the
  * comparison, both for first and second the addresses of the PLVertex objects
  * are sorted ascending. After that, the mFirst members are compared, then
  * the mSecond members. Only when one value of first is smaller than the
  * corresponding value of second, true is returned, false otherwise.
  *
  * @param first const pointer to the first PLAdjacentVertexPair object in the comparison
  * @param second const pointer to the second PLAdjacentVertexPair object in the comparison
  * @return only when one value of first is smaller than the corresponding value of second, true is returned, false otherwise
  */
bool PLAdjacentVertexPairPCompare::operator()(const PLAdjacentVertexPair *first, const PLAdjacentVertexPair *second) const
{
	PLAdjacentVertexPairCompare compare;
	return compare( *first, *second );
}

/**
  *	Constructs a new PLPlanarPolygon3dsOptimization object with the given arguments
  *
  * @param imageNamesToBlobsMap pointer to the map, that maps image names (not paths!) to sets with all the pointers to PLBlob objects that correspond to one particular image
  * @param options options for the PLPlanarPolygon3dsOptimization object
  */
PLPlanarPolygon3dsOptimization::PLPlanarPolygon3dsOptimization(
	map<string, set<PLBlob*>> *materialNamesToBlobsMap,
	const PLPlanarPolygon3dsOptimizationOptions &options) : PLNonLinearLeastSquaresSolver( options )
{
	mMaterialNamesToBlobsMap = materialNamesToBlobsMap;

	mOptions = options;

	mNumberOfVariables = 0;

	Initialize();
}

/**
  *	Destroys the PLPlanarPolygon3dsOptimization object
  */
PLPlanarPolygon3dsOptimization::~PLPlanarPolygon3dsOptimization( void )
{
/*	if (mBiggestDifferenceVertexPair) delete mBiggestDifferenceVertexPair;*/
}

/**
  *	Is called by BuildLinearEquationSystem() to fill the matrix A and vector b
  */
void PLPlanarPolygon3dsOptimization::VertexToOriginalVertexTerm( void )
{
	double f_i = 0.0;
	double g_i1_j = 0.0;
	double g_i2_j = 0.0;

	double g_i_j1 = 0.0;
	double g_i_j2 = 0.0;

	double f_i_j2 = 0.0;
	double f_i_j1 = 0.0;

	double f_i_j = 0.0;

	double standard_coefs[] = { 1.0, -1.0 };

	double sqrt_second_term_weight = sqrt( mOptions.mSecondTermWeight );
	double sqrt_third_term_weight = sqrt( mOptions.mThirdTermWeight );

	vector<PLuint> variables;
	vector<double> coefficients( &standard_coefs[0], &standard_coefs[2] );
	vector<double> third_term_coefs( 1, 1.0 );

	// Lines for the First Term

	// For every image
// 	for (map<string, set<PLBlob*>>::const_iterator image_it = mImageNamesToBlobsMap->begin();
// 		image_it != mImageNamesToBlobsMap->end(); image_it++)
// 	{
// 		// For every blob inside the image
// 		for (set<PLBlob*>::const_iterator blob_it = image_it->second.begin();
// 			blob_it != image_it->second.end(); blob_it++)
// 		{
// 			// For every outline edge inside the blob
// 			for(std::set<PLEdge*, PLEdge::PLEdgeVIPCompare>::const_iterator vertex_it = (*blob_it)->GetEdgeSet().begin();
// 				vertex_it != (*blob_it)->GetEdgeSet().end(); vertex_it++)

	// For every edge
	for (vector<PLEdge*>::const_iterator edge_it = mEdges.begin(); edge_it != mEdges.end(); edge_it++)
	{
		variables.clear();

		switch (mOptions.mChannel)
		{
			case PL_CHANNEL_RED:
			{
				g_i1_j = (*edge_it)->From().GetLevellingR();
				g_i2_j = (*edge_it)->To().GetLevellingR();
				break;
			}

			case PL_CHANNEL_GREEN:
			{
				g_i1_j = (*edge_it)->From().GetLevellingG();
				g_i2_j = (*edge_it)->To().GetLevellingG();
				break;
			}

			case PL_CHANNEL_BLUE:
			{
				g_i1_j = (*edge_it)->From().GetLevellingB();
				g_i2_j = (*edge_it)->To().GetLevellingB();
				break;
			}
		}

		variables.push_back( mVertexAdressToIndexMap[&(*edge_it)->From()] );
		variables.push_back( mVertexAdressToIndexMap[&(*edge_it)->To()] );

		f_i = g_i1_j - g_i2_j;

		FillJTJ( variables, coefficients, 1.0, f_i );
	}

	// Lines for the Second Term
	for (set<PLAdjacentVertexPair, PLAdjacentVertexPairCompare>::const_iterator adjacent_vertexes_it = mAdjacentVertexPairs.begin();
		adjacent_vertexes_it != mAdjacentVertexPairs.end(); adjacent_vertexes_it++)
	{
		variables.clear();

		switch (mOptions.mChannel)
		{
			case PL_CHANNEL_RED:
			{
				g_i_j1 = adjacent_vertexes_it->mFirst->GetLevellingR();
				g_i_j2 = adjacent_vertexes_it->mSecond->GetLevellingR();

				f_i_j1 = adjacent_vertexes_it->mFirst->GetOriginalR();
				f_i_j2 = adjacent_vertexes_it->mSecond->GetOriginalR();
				break;
			}

			case PL_CHANNEL_GREEN:
			{
				g_i_j1 = adjacent_vertexes_it->mFirst->GetLevellingG();
				g_i_j2 = adjacent_vertexes_it->mSecond->GetLevellingG();

				f_i_j1 = adjacent_vertexes_it->mFirst->GetOriginalG();
				f_i_j2 = adjacent_vertexes_it->mSecond->GetOriginalG();
				break;
			}

			case PL_CHANNEL_BLUE:
			{
				g_i_j1 = adjacent_vertexes_it->mFirst->GetLevellingB();
				g_i_j2 = adjacent_vertexes_it->mSecond->GetLevellingB();

				f_i_j1 = adjacent_vertexes_it->mFirst->GetOriginalB();
				f_i_j2 = adjacent_vertexes_it->mSecond->GetOriginalB();
				break;
			}
		}

		variables.push_back( mVertexAdressToIndexMap[adjacent_vertexes_it->mFirst] );
		variables.push_back( mVertexAdressToIndexMap[adjacent_vertexes_it->mSecond] );

		f_i = g_i_j1 - g_i_j2 - f_i_j2 + f_i_j1;

		FillJTJ( variables, coefficients, sqrt_second_term_weight, f_i );
	}

	// Lines for the Third Term
	for (vector<PLVertex*>::const_iterator vertex_it = mVertexes.begin();
		vertex_it != mVertexes.end(); vertex_it++)
	{
		variables.clear();

		switch (mOptions.mChannel)
		{
		case PL_CHANNEL_RED:
			{
				f_i_j = (*vertex_it)->GetLevellingR();
				break;
			}

		case PL_CHANNEL_GREEN:
			{
				f_i_j = (*vertex_it)->GetLevellingG();
				break;
			}

		case PL_CHANNEL_BLUE:
			{
				f_i_j = (*vertex_it)->GetLevellingB();
				break;
			}
		}

		variables.push_back( mVertexAdressToIndexMap[(*vertex_it)] );

		f_i = f_i_j;

		FillJTJ( variables, third_term_coefs, sqrt_third_term_weight, f_i );
	}

//	TODO
//
// 	for (PLuint i = 1; i <=m; i++)
// 	{
// 		g_i = g[i];
// 		f_i = g_i - c;
// 
// 		FillJTJ(new int[] { 0 },
// 			new double[] { 1 },
// 			weight, 
// 			f_i);
// 	}
}

/**
  *	Is called by Residual() and returns the sum of the residuals corresponding to
  * first and second term in the optimization equation.
  *
  * @return sum of the residuals corresponding to first and second term in the optimization equation
  */
double PLPlanarPolygon3dsOptimization::VertexToOriginalVertexResidual( void ) const
{
	return FirstTermResidual() + SecondTermResidual() + ThirdTermResidual();

	//
	// 	double residual = 0.0;
	// 
	// 	for (int i = 1; i <=m; i++)
	// 	{
	// 		g_i = g[i];
	// 		f_i = g_i - c;    
	// 
	// 		residual += (f_i * f_i);
	// 	}
	// 
	// 	return residual * weight * weight;
}

/**
  *	Calls VertexToOriginalVertexTerm() to build the linear equation system by
  * filling the matrix A and vector b.
  */
void PLPlanarPolygon3dsOptimization::BuildLinearEquationSystem()
{
	VertexToOriginalVertexTerm();
}

/**
  *	Returns the current residual, which is the difference between A * x~
  * (with x~ as the current solution) and A * x (with x as the exact solution)
  */
double PLPlanarPolygon3dsOptimization::Residual() const
{
	double residual = VertexToOriginalVertexResidual();

	return residual * 0.5;
}

/**
  *	Returns the residual corresponding to the first term in the optimization equation
  *
  * @return residual corresponding to the first term in the optimization equation
  */
double PLPlanarPolygon3dsOptimization::FirstTermResidual( void ) const
{
	double first_term = 0.0;
	double residual = 0.0;

	double g_i1_j = 0.0;
	double g_i2_j = 0.0;

	double dif = 0.0;

	// First term

	// For every image
// 	for (map<string, set<PLBlob*>>::const_iterator image_it = mImageNamesToBlobsMap->begin();
// 		image_it != mImageNamesToBlobsMap->end(); image_it++)
// 	{
// 		// For every blob inside the image
// 		for (set<PLBlob*>::const_iterator blob_it = image_it->second.begin();
// 			blob_it != image_it->second.end(); blob_it++)
// 		{
// 			// For every outline edge inside the blob
// 			for(std::set<PLEdge*, PLEdge::PLEdgeVIPCompare>::const_iterator vertex_it = (*blob_it)->GetEdgeSet().begin();
// 				vertex_it != (*blob_it)->GetEdgeSet().end(); vertex_it++)

	// For every edge
	for (vector<PLEdge*>::const_iterator edge_it = mEdges.begin(); edge_it != mEdges.end(); edge_it++)
	{
		switch (mOptions.mChannel)
		{
		case PL_CHANNEL_RED:
			{
				g_i1_j = (*edge_it)->From().GetLevellingR();
				g_i2_j = (*edge_it)->To().GetLevellingR();
				break;
			}

		case PL_CHANNEL_GREEN:
			{
				g_i1_j = (*edge_it)->From().GetLevellingG();
				g_i2_j = (*edge_it)->To().GetLevellingG();
				break;
			}

		case PL_CHANNEL_BLUE:
			{
				g_i1_j = (*edge_it)->From().GetLevellingB();
				g_i2_j = (*edge_it)->To().GetLevellingB();
				break;
			}
		}

		dif = g_i1_j - g_i2_j;

		first_term += dif * dif;
	}

	residual = first_term;

	return residual;
}

/**
  *	Returns the residual corresponding to the second term in the optimization equation
  *
  * @return residual corresponding to the second term in the optimization equation
  */
double PLPlanarPolygon3dsOptimization::SecondTermResidual( void ) const
{
	double dif = 0.0;
	double g_i_j1 = 0.0;
	double g_i_j2 = 0.0;
	double f_i_j1 = 0.0;
	double f_i_j2 = 0.0;

	double second_term = 0.0;
	double residual = 0.0;

	// Second Term
	for (set<PLAdjacentVertexPair, PLAdjacentVertexPairCompare>::const_iterator adjacent_vertexes_it = mAdjacentVertexPairs.begin();
		adjacent_vertexes_it != mAdjacentVertexPairs.end(); adjacent_vertexes_it++)
	{
		switch (mOptions.mChannel)
		{
			case PL_CHANNEL_RED:
			{
				g_i_j1 = adjacent_vertexes_it->mFirst->GetLevellingR();
				g_i_j2 = adjacent_vertexes_it->mSecond->GetLevellingR();

				f_i_j1 = adjacent_vertexes_it->mFirst->GetOriginalR();
				f_i_j2 = adjacent_vertexes_it->mSecond->GetOriginalR();
				break;
			}

			case PL_CHANNEL_GREEN:
			{
				g_i_j1 = adjacent_vertexes_it->mFirst->GetLevellingG();
				g_i_j2 = adjacent_vertexes_it->mSecond->GetLevellingG();

				f_i_j1 = adjacent_vertexes_it->mFirst->GetOriginalG();
				f_i_j2 = adjacent_vertexes_it->mSecond->GetOriginalG();
				break;
			}

			case PL_CHANNEL_BLUE:
			{
				g_i_j1 = adjacent_vertexes_it->mFirst->GetLevellingB();
				g_i_j2 = adjacent_vertexes_it->mSecond->GetLevellingB();

				f_i_j1 = adjacent_vertexes_it->mFirst->GetOriginalB();
				f_i_j2 = adjacent_vertexes_it->mSecond->GetOriginalB();
				break;
			}
		}

		dif = g_i_j1 - g_i_j2 - f_i_j2 + f_i_j1;

		second_term += dif * dif;
	}

	residual = mOptions.mSecondTermWeight * second_term;

	return residual;
}

/**
  *	Returns the residual corresponding to the third term in the optimization equation
  *
  * @return residual corresponding to the third term in the optimization equation
  */
double PLPlanarPolygon3dsOptimization::ThirdTermResidual( void ) const
{
	double f_i = 0.0;

	double third_term = 0.0;
	double residual = 0.0;

	// Third Term
	for (vector<PLVertex*>::const_iterator vertex_it = mVertexes.begin();
		vertex_it != mVertexes.end(); vertex_it++)
	{
		switch (mOptions.mChannel)
		{
		case PL_CHANNEL_RED:
			{
				f_i = (*vertex_it)->GetLevellingR();
				break;
			}

		case PL_CHANNEL_GREEN:
			{
				f_i = (*vertex_it)->GetLevellingG();
				break;
			}

		case PL_CHANNEL_BLUE:
			{
				f_i = (*vertex_it)->GetLevellingB();
				break;
			}
		}

		third_term += f_i * f_i;
	}

	residual = mOptions.mThirdTermWeight * third_term;

	return residual;
}

/**
  *	Sets the current channel for the levelling function calculation
  *
  * @param channel current channel for the levelling function calculation
  */
void PLPlanarPolygon3dsOptimization::SetCurrentChannel( PLChannel channel )
{
	mOptions.mChannel = channel;
}

/**
  *	Initializes the solver (allocates memory, calculates mean grey values for
  * the channels of the image etc.)
  */
void PLPlanarPolygon3dsOptimization::Initialize()
{
	set<PLVertex *> all_vertexes;
	set<PLEdge *, PLEdge::PLEdgeVIAndTIPCompare> all_edges;

	const set<PLVertex *> *adjacent_vertexes = 0;

	// For every material-image
	for (map<string, set<PLBlob*>>::const_iterator image_it = mMaterialNamesToBlobsMap->begin();
		image_it != mMaterialNamesToBlobsMap->end(); image_it++)
	{
		// For every blob inside the image
		for (set<PLBlob*>::const_iterator blob_it = image_it->second.begin();
			blob_it != image_it->second.end(); blob_it++)
		{
			// For every triangle inside the blob
			for (vector<PLTriangle*>::const_iterator triangle_it = (*blob_it)->GetTriangleVector().begin();
				triangle_it != (*blob_it)->GetTriangleVector().end(); triangle_it++)
			{
				// For every edge inside the Triangle
				for (vector<PLEdge*>::const_iterator edge_it = (*triangle_it)->GetEdges().begin();
					edge_it != (*triangle_it)->GetEdges().end(); edge_it++)
				{
					all_edges.insert( (*edge_it) );

					all_vertexes.insert( &(*edge_it)->From() );

					adjacent_vertexes = (*edge_it)->From().GetAdjacentVertexes();

					if (adjacent_vertexes)
					{
						for (set<PLVertex *>::const_iterator adjacent_vertex_it = adjacent_vertexes->begin();
							adjacent_vertex_it != adjacent_vertexes->end(); adjacent_vertex_it++)
						{
							PLAdjacentVertexPair p( &(*edge_it)->From(), *adjacent_vertex_it );

							mAdjacentVertexPairs.insert( p );
						}
					}
				}
			}
		}
	}

	// Store all variables in a vector
	mVertexes.assign( all_vertexes.begin(), all_vertexes.end() );
	mNumberOfVariables = mVertexes.size();

	// Store all edges in a vector
	mEdges.assign( all_edges.begin(), all_edges.end() );

	cout << "Num edges for solving: " << mEdges.size() << endl;

	mOriginalMeanGreyValues[R] = 0.0;
	mOriginalMeanGreyValues[G] = 0.0;
	mOriginalMeanGreyValues[B] = 0.0;

	double color_values[3];
	double min_color_values[3] = { 1.0e9, 1.0e9, 1.0e9 };
	double max_color_values[3] = { -1.0e9, -1.0e9, -1.0e9 };

	// Fill the mVertexAdressToIndexMap and find original mean grey values
	PLuint i = 0U;
	PLuint j = 0U;
	for (vector<PLVertex*>::iterator it = mVertexes.begin();
		it != mVertexes.end(); it++)
	{
		mVertexAdressToIndexMap.insert( make_pair( *it, i++ ) );

		memcpy( color_values, (*it)->GetOriginalBGR(), 3 * sizeof(double) );

		for (j = 0; j < 3; j++)
		{
			if (color_values[j] < min_color_values[j]) min_color_values[j] = color_values[j];
			if (color_values[j] > max_color_values[j]) max_color_values[j] = color_values[j];
		}

		mOriginalMeanGreyValues[R] += color_values[R];
		mOriginalMeanGreyValues[G] += color_values[G];
		mOriginalMeanGreyValues[B] += color_values[B];
	}

	mOriginalMeanGreyValues[R] /= mVertexes.size();
	mOriginalMeanGreyValues[G] /= mVertexes.size();
	mOriginalMeanGreyValues[B] /= mVertexes.size();

	mOriginalColorRanges[R] = max_color_values[R] - min_color_values[R];
	mOriginalColorRanges[G] = max_color_values[G] - min_color_values[G];
	mOriginalColorRanges[B] = max_color_values[B] - min_color_values[B];

	PLNonLinearLeastSquaresSolver::Initialize();

// 	NumberOfVariables = anzahl der g_i's;
// 
// 		g_i = 0; for all i;
// 
// 	base.Initialize();
}

/**
  *	Updates the variables (=vertexes) by adding the gradient to the current
  * vertex vector.
  *
  * @param gradient const reference to a vector containing the gradient for the update step
  */
void PLPlanarPolygon3dsOptimization::UpdateVariables( const std::vector<double> &gradient )
{
	assert( gradient.size() == mVertexes.size() );

	PLuint i = 0;

	for (vector<PLVertex*>::const_iterator vertex_it = mVertexes.begin();
		vertex_it != mVertexes.end(); vertex_it++)
	{
		switch(mOptions.mChannel)
		{
			case PL_CHANNEL_RED:
			{
				(*vertex_it)->SetLevellingR( (*vertex_it)->GetLevellingR() + gradient[i] );
				break;
			}

			case PL_CHANNEL_GREEN:
			{
				(*vertex_it)->SetLevellingG( (*vertex_it)->GetLevellingG() + gradient[i] );
				break;
			}

			case PL_CHANNEL_BLUE:
			{
				(*vertex_it)->SetLevellingB( (*vertex_it)->GetLevellingB() + gradient[i] );
				break;
			}
		}

		i++;
	}

// 	Requires.That(gradient.Count == NumberOfVariables);
// 
// 	for (int i = 1; i <= m; i++)
// 		g[i] += gradient[i];
}

/**
  *	Stores the current levelling values of the vertexes in the given vector
  *
  * @param[out] backup vector to store the current levelling values of the vertexes
  */
void PLPlanarPolygon3dsOptimization::BackupVariables( std::vector<double> *backup )
{
	assert( backup->size() == mVertexes.size() );

	PLuint i = 0;

	for (vector<PLVertex*>::const_iterator vertex_it = mVertexes.begin();
		vertex_it != mVertexes.end(); vertex_it++)
	{
		switch(mOptions.mChannel)
		{
			case PL_CHANNEL_RED:
			{
				(*backup)[i] = (*vertex_it)->GetLevellingR();
				break;
			}

			case PL_CHANNEL_GREEN:
			{
				(*backup)[i] = (*vertex_it)->GetLevellingG();
				break;
			}

			case PL_CHANNEL_BLUE:
			{
				(*backup)[i] = (*vertex_it)->GetLevellingB();
				break;
			}
		}

		i++;
	}
// 	backup = new Vector<double>(NumberOfVariables);
// 
// 	for (int i = 1; i <= m; i++)
// 		backup[i] = g[i];
}

/**
  *	Restores the levelling values of the vertexes by overwriting the current
  * values with the ones in the backup vector
  *
  * @param backup const reference to a vector with the backup-values
  */
void PLPlanarPolygon3dsOptimization::RestoreVariables( const std::vector<double> &backup )
{
	assert( backup.size() == mVertexes.size() );

	PLuint i = 0;

	for (vector<PLVertex*>::const_iterator vertex_it = mVertexes.begin();
		vertex_it != mVertexes.end(); vertex_it++)
	{
		switch(mOptions.mChannel)
		{
		case PL_CHANNEL_RED:
			{
				(*vertex_it)->SetLevellingR( backup[i] );
				break;
			}

		case PL_CHANNEL_GREEN:
			{
				(*vertex_it)->SetLevellingG( backup[i] );
				break;
			}

		case PL_CHANNEL_BLUE:
			{
				(*vertex_it)->SetLevellingB( backup[i] );
				break;
			}
		}

		i++;
	}
// 	Requires.That(backup.Count == NumberOfVariables);
// 
// 	for (int i = 1; i <= m; i++)
// 		g[i] = backup[i];
}

/**
  *	Calculates the mean grey value for the current channel after the levelling
  * process and tries to find an offset value that is added to the levelling
  * value of all vertexes to restore the original mean grey value.
  */
void PLPlanarPolygon3dsOptimization::FinalizeSolver()
{
// 
// 	neue farbwerte = alte farbwerte + g;
//
	const double MIN_VALUE = 0.0;
	const double MAX_VALUE = 1.0;

	double levelling_value = MIN_VALUE;
	double min_levelling = -1000000.0;

	double new_mean_grey_value = 0.0;

	double current_value = MIN_VALUE;

	cout << "Finalizing solver for the " << GetCurrentChannelName() << " channel..." << endl;

	double color_value = 0.0;
	double original_min_color_value = 1.0e9;
	double original_max_color_value = -1.0e9;
	double min_color_value = 1.0e9;
	double max_color_value = -1.0e9;
	double color_range = 0.0;
	double shift_value = 0.0;
	vector<double> color_values;
	color_values.reserve( mVertexes.size() );

	const double& (PLVertex::*levelling_color_func)( void ) const;
	const double& (PLVertex::*original_color_func)( void ) const;
	void (PLVertex::*set_levelling_color_func)( const double& );
	PLuint color_index = 0U;

	switch (mOptions.mChannel)
	{
	case PL_CHANNEL_RED:
		levelling_color_func = &PLVertex::GetLevellingR;
		original_color_func = &PLVertex::GetOriginalR;
		set_levelling_color_func = &PLVertex::SetLevellingR;
		color_index = R;
		break;
	case PL_CHANNEL_GREEN:
		levelling_color_func = &PLVertex::GetLevellingG;
		original_color_func = &PLVertex::GetOriginalG;
		set_levelling_color_func = &PLVertex::SetLevellingG;
		color_index = G;
		break;
	case PL_CHANNEL_BLUE:
		levelling_color_func = &PLVertex::GetLevellingB;
		original_color_func = &PLVertex::GetOriginalB;
		set_levelling_color_func = &PLVertex::SetLevellingB;
		color_index = B;
		break;
	}

// 	final_level = 0.5 * (
// 		static_cast<double>(mBiggestDifferenceVertexPair->mFirst->GetOriginalR()) +
// 		static_cast<double>(mBiggestDifferenceVertexPair->mSecond->GetOriginalR())
// 		);
// 	levelling_value = final_level -
// 		mBiggestDifferenceVertexPair->mFirst->GetLevellingR() -
// 		mBiggestDifferenceVertexPair->mFirst->GetOriginalR();

	for (vector<PLVertex*>::iterator it = mVertexes.begin();
		it != mVertexes.end(); it++)
	{
		color_value = ((*it)->*original_color_func)();
		
		if (color_value < original_min_color_value)
			original_min_color_value = color_value;

		if (color_value > original_max_color_value)
			original_max_color_value = color_value;

		color_value += ((*it)->*levelling_color_func)();
		color_values.push_back( color_value );

		if (color_value < min_color_value) min_color_value = color_value;
		if (color_value > max_color_value) max_color_value = color_value;

		//new_mean_grey_value += color_value;
	}
	//new_mean_grey_value /= mVertexes.size();

	color_range = max_color_value - min_color_value;

	//levelling_value = mOriginalMeanGreyValues[R] - new_mean_grey_value;

// 			cout << "Chosen offset-value: " << levelling_value << endl;
	cout << "Grey value range before leveling: [" << original_min_color_value << " " << original_max_color_value << "]" << endl;
	cout << "Grey value range after leveling: [" << min_color_value << " " << max_color_value << "]" << endl;

	// Scale the color range if necessary
// 	if (color_range > 1.0)
// 	{
// 		for (vector<double>::iterator color_it = color_values.begin();
// 			color_it != color_values.end(); color_it++)
// 		{
// 			*color_it /= color_range;
// 		}
// 
// 		max_color_value /= color_range;
// 		min_color_value /= color_range;
// 
// 		color_range = 1.0;
// 
// 		cout << "Grey value range after scaling in the color range: [" << min_color_value << " " << max_color_value << "]" << endl;
// 	}
// 
// 	// Translate the color range if necessary
// 	if (min_color_value < 0.0)
// 	{
// 		for (vector<double>::iterator color_it = color_values.begin();
// 			color_it != color_values.end(); color_it++)
// 		{
// 			*color_it -= min_color_value;
// 		}
// 
// 		max_color_value -= min_color_value;
// 		min_color_value = 0.0;
// 
// 		cout << "Grey value range after translating in the color range: [" << min_color_value << " " << max_color_value << "]" << endl;
// 	}
// 	else if (max_color_value > 1.0)
// 	{
// 		shift_value = max_color_value - 1.0;
// 		for (vector<double>::iterator color_it = color_values.begin();
// 			color_it != color_values.end(); color_it++)
// 		{
// 			*color_it -= shift_value;
// 		}
// 
// 		max_color_value = 1.0;
// 		min_color_value -= shift_value;
// 
// 		cout << "Grey value range after translating in the color range: [" << min_color_value << " " << max_color_value << "]" << endl;
// 	}
// 
// 	new_mean_grey_value = 0.0;
// 	for (vector<double>::iterator color_it = color_values.begin();
// 		color_it != color_values.end(); color_it++)
// 	{
// 		new_mean_grey_value += *color_it;
// 	}
// 
// 	new_mean_grey_value /= color_values.size();
// 
// 	cout << "Mean grey value before levelling: " << mOriginalMeanGreyValues[color_index] << endl;
// 	cout << "Color value range before levelling: " << mOriginalColorRanges[color_index] << endl << endl;
// 	cout << "Mean grey value after levelling: " << new_mean_grey_value << endl;
// 	cout << "Color value range after levelling: " << color_range << endl << endl;
// 
// 	PLuint i = 0U;
// 
// 	//Set the found levelling
// 	for (vector<PLVertex*>::iterator vertex_it = mVertexes.begin();
// 		vertex_it != mVertexes.end(); vertex_it++)
// 	{
// 		((*vertex_it)->*set_levelling_color_func)( color_values[i++] - ((*vertex_it)->*original_color_func)() );
// 	}


///////////////////////////////////////////////////////////////////////////////
// Old method

	// Test if the levelling leads to values < 0
// 	for (vector<PLVertex*>::iterator vertex_it = mVertexes.begin();
// 		vertex_it != mVertexes.end(); vertex_it++)
// 	{
// 		current_value = ((*vertex_it)->*original_color_func)() + ((*vertex_it)->*levelling_color_func)() + levelling_value;
// 		if (current_value < MIN_VALUE)
// 		{
// 			// Value out of bounds, < 0
// 			levelling_value = levelling_value - current_value;
// 			min_levelling = levelling_value;
// 
// 			cout << "Changed offset-value to: " << levelling_value << ", (value < 0.0)" << endl;
// 		}
// 	}

	// Test if the levelling leads to values > 1.0
// 	for (vector<PLVertex*>::iterator vertex_it = mVertexes.begin();
// 		vertex_it != mVertexes.end(); vertex_it++)
// 	{
// 		current_value = ((*vertex_it)->*original_color_func)() + ((*vertex_it)->*levelling_color_func)() + levelling_value;
// 		if (current_value > MAX_VALUE)
// 		{
// 			// Value out of bounds, > 1.0
// 			levelling_value = levelling_value - current_value + MAX_VALUE;
// 
// 			cout << "Changed offset-value to: " << levelling_value << ", (value > 1.0)" << endl;
// 
// 			if (levelling_value < min_levelling)
// 			{
// 				cout << "WARNING: No levelling function without color truncation possible!" << endl;
// 				break;
// 			}
// 		}
// 	}

	// Set the found levelling
// 	for (vector<PLVertex*>::iterator vertex_it = mVertexes.begin();
// 		vertex_it != mVertexes.end(); vertex_it++)
// 	{
// 		((*vertex_it)->*set_levelling_color_func)( ((*vertex_it)->*levelling_color_func)() + levelling_value );
// 	}
// 	cout << "Mean grey value after corrected levelling: " << new_mean_grey_value + levelling_value  << endl;
}

/**
  *	Returns the current channel name (Red, Green or Blue)
  *
  * @return current channel name (Red, Green or Blue)
  */
string PLPlanarPolygon3dsOptimization::GetCurrentChannelName( void ) const
{
	switch (mOptions.mChannel)
	{
		case PL_CHANNEL_RED:
			return "Red";
		case PL_CHANNEL_GREEN:
			return "Green";
		case PL_CHANNEL_BLUE:
			return "Blue";
	}

	return "";
}